'''
@file:    qr_color_circle_serial_success.py
@company: UniTongChuang
@author:  Boris
@date:    2025-1-14
@description:
    二维码识别流程：当接收到下位机发送的“qr_detect_start”后，视觉模块开始检测图片中的二维码信息
    并将该信息发送给下位机Mega2560，当Mega2560接收到二维码信息后，确认二维码无误后，下位机给上
    未机发送“qr_detect_success”，当上位机接收到该消息后，给下位机发送“qr_success”，当下位机
    接收到上位机发送的“qr_success"后，下位机结束循环，同时上位机也结束二维码检测。
'''

#载入相关模块
import sensor, image, time, lcd, time, gc, cmath
from hiwonder import hw_uart, hw_led , fill_light
import binascii
from maix import KPU


#创建LED控制对象
led = hw_led()
#创建补光灯控制对象
f_led = fill_light()
#定义串口对象
serial = hw_uart()
COLOR_MODE = 1
QR_MODE = 2
CIRCLE_CENTER = 3
JUDGE_MODE = 4
COLORRING_MODE = 5
COLOR_DATA_MODE = 6
COLORRING_DATA_MODE = 7

##################################### send_func begin #####################################

#CRC校验参数，不可修改，否则数据通讯会出错
crc8_table = [
    0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, 65,
    157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96, 130, 220,
    35, 125, 159, 193, 66, 28, 254, 160, 225, 191, 93, 3, 128, 222, 60, 98,
    190, 224, 2, 92, 223, 129, 99, 61, 124, 34, 192, 158, 29, 67, 161, 255,
    70, 24, 250, 164, 39, 121, 155, 197, 132, 218, 56, 102, 229, 187, 89, 7,
    219, 133, 103, 57, 186, 228, 6, 88, 25, 71, 165, 251, 120, 38, 196, 154,
    101, 59, 217, 135, 4, 90, 184, 230, 167, 249, 27, 69, 198, 152, 122, 36,
    248, 166, 68, 26, 153, 199, 37, 123, 58, 100, 134, 216, 91, 5, 231, 185,
    140, 210, 48, 110, 237, 179, 81, 15, 78, 16, 242, 172, 47, 113, 147, 205,
    17, 79, 173, 243, 112, 46, 204, 146, 211, 141, 111, 49, 178, 236, 14, 80,
    175, 241, 19, 77, 206, 144, 114, 44, 109, 51, 209, 143, 12, 82, 176, 238,
    50, 108, 142, 208, 83, 13, 239, 177, 240, 174, 76, 18, 145, 207, 45, 115,
    202, 148, 118, 40, 171, 245, 23, 73, 8, 86, 180, 234, 105, 55, 213, 139,
    87, 9, 235, 181, 54, 104, 138, 212, 149, 203, 41, 119, 244, 170, 72, 22,
    233, 183, 85, 11, 136, 214, 52, 106, 43, 117, 151, 201, 74, 20, 246, 168,
    116, 42, 200, 150, 21, 75, 169, 247, 182, 232, 10, 84, 215, 137, 107, 53
]

#CRC校验函数
def checksum_crc8(data):
    # 校验
    check = 0
    for b in data:
        check = crc8_table[check ^ b]
    return check & 0x00FF

#功能号
class FuncTag:
    START = 0
    FIND_QRCODES  = 3
    FIND_MULTI_COLOR  = 0x10
    FIND_OBJECT  = 8
    K210_FIND_APRILTAGS = 4

#字符串转int
def str_2_int(data_str):
    bb = binascii.hexlify(data_str)
    bb = str(bb)[2:-1]
    int_h = int(bb[0])*16
    int_l = int(bb[1],16)
    return int_h+int_l


def send_data(mode,x,y,w,h,msg):
    '''
    0xAA  0x55  功能号  数据长度  data  CRC
    '''
    START_1 = 0xAA
    START_2 = 0x55
    if mode == COLOR_MODE:
        FUNC_num = FuncTag.FIND_MULTI_COLOR #颜色功能编号
    elif mode == QR_MODE:
        FUNC_num = FuncTag.FIND_QRCODES #二维码功能编号

    elif mode == CIRCLE_CENTER:
        FUNC_num = 2

    elif mode == JUDGE_MODE:
        FUNC_num = 4

    elif mode == COLORRING_MODE:
        FUNC_num = FuncTag.FIND_OBJECT

    elif mode == COLOR_DATA_MODE:
        FUNC_num = FuncTag.FIND_MULTI_COLOR

    elif mode == COLORRING_DATA_MODE:
        FUNC_num = FuncTag.FIND_OBJECT
    Length = 0  #数据长度
    crc = 0 #校验位
    data = [] #数据组

    #参数都为0
    if x==0 and y==0 and w==0 and h ==0:
        pass
    else:
        #x(小端模式)
        low = x & 0xFF #低位
        high = x >> 8& 0xFF #高位
        data.append(low)
        data.append(high)

        #y(小端模式)
        low = y & 0xFF #低位
        high = y >> 8& 0xFF #高位
        data.append(low)
        data.append(high)

        #w(小端模式)
        low = w & 0xFF #低位
        high = w >> 8& 0xFF #高位
        data.append(low)
        data.append(high)

        #h(小端模式)
        low = h & 0xFF #低位
        high = h >> 8& 0xFF #高位
        data.append(low)
        data.append(high)

    #msg
    if msg != None:
        for i in range(len(msg)):
            msg_int = str_2_int(msg[i])
            data.append(msg_int)

    Length += len(data)
    send_buf = [FUNC_num,Length]
    for i in range(len(data)):
        send_buf.append(data[i])

    #进行CRC运算
    crc = checksum_crc8(send_buf)

    send_buf.insert(0,START_1) #插入协议头1
    send_buf.insert(1,START_2) #插入协议头2
    send_buf.append(crc) #加入CRC校验码

    #print(send_buf) #打印数据
    serial.send_bytearray(send_buf) #发送数据



##################################### send_func end #####################################
def qr_detection_section():
    #clock.tick()#用于计算帧率的函数，这里表示开始计时
    #img = sensor.snapshot()#从传感器捕获一张图像
    #遍历图像中找到的二维码
    try:
        for code in img.find_qrcodes():
            #绘制矩形和识别出的信息
            img.draw_rectangle(code.rect(), color = 127, thickness=3)
            img.draw_string(0, 0, code.payload(), color=(255, 0, 0), scale=8.5)
            #将方框数据与消息赋值
            send_x = code.x()
            send_y = code.y()
            send_w = code.w()
            send_h = code.h()
            send_msg = code.payload()
        # 当消息标志不为 '' 时，发送数据
        if send_msg != "":
            #发送数据
            send_data(QR_MODE,send_x,send_y,send_w,send_h,send_msg)
            send_msg = ""   # 清空消息变量
            for i in range(2):
                f_led.fill_onoff(1)
                time.sleep(0.01)
                f_led.fill_onoff(0)
                time.sleep(0.01)
    except:
        print("nothing to do")
    ##显示在LCD上
    #lcd.display(img)


def color_detection_section():
    try:
        for color_idx, threshold in enumerate(color_thresholds):
            #遍历图像中找到的颜色区块
            blobs = img.find_blobs([threshold], pixels_threshold=100, area_threshold=100, merge=True, margin=10)
            #是否找到
            if blobs:
                area_max = 0
                blob_max = blobs[0]
                #寻找最大的颜色块
                for blob in blobs:
                    area = blob.w()*blob.h()
                    if(area_max < area):
                        blob_max = blob
                        area_max = area
                if area_max < 2000:
                    continue
                #画方框
                img.draw_rectangle(blob_max.rect())
                img.draw_cross(blob_max.cx(), blob_max.cy())
                img.draw_string(blob_max.cx() + 10, blob_max.cy() - 10, color_strings[color_idx], color=(255, 255, 255))
                #将方框数据与消息赋值
                send_x = blob_max.x()
                send_y = blob_max.y()
                send_w = blob_max.w()
                send_h = blob_max.h()
                #发送数据
                #send_color_data(send_x,send_y,send_w,send_h,color_strings[color_idx])
                send_data(COLOR_MODE,send_x,send_y,send_w,send_h,color_strings[color_idx])
                for i in range(2):
                    f_led.fill_onoff(1)
                    time.sleep(0.01)
                    f_led.fill_onoff(0)
                    time.sleep(0.01)
    except:
        print("nothing to do")
    ##显示在LCD上
    #lcd.display(img)
    ##打印帧率
    ##print(clock.fps())

def color_ring_detect():
    gc.collect()
    clock.tick()
    img = sensor.snapshot()
    kpu.run_with_output(img)
    dect = kpu.regionlayer_yolo2()
    #fps = clock.fps()
    if len(dect) > 0:
        for l in dect :
            a = img.draw_rectangle(l[0],l[1],l[2],l[3],color=(0,255,0))

            info = "%s %.3f" % (obj_name[l[4]], l[5])
            a = img.draw_string(l[0],l[1],info,color=(255,0,0),scale=2.0)
            #print(info)
            send_x = l[0]
            send_y = l[1]
            send_w = l[2]
            send_h = l[3]
            send_msg = obj_name[l[4]]
            send_data(COLORRING_MODE, send_x,send_y,send_w,send_h,send_msg)
            del info

    #a = img.draw_string(0, 0, "%2.1ffps" %(fps),color=(0,60,255),scale=2.0)
    #lcd.display(img)


def position_judge():
    line_value = None
    img = sensor.snapshot()
    img.morph(1, [+2, +1, +0, +1, +1, -1, +0, -1, -2])
    #img.dilate(1)
    lines = img.find_lines(threshold=6000, theta_margin=25, rho_margin=25)
    if lines:
        for l in lines:
            img.draw_line(l.line(), color=(255, 0, 0))
            line_value = l
            send_x = line_value[0]
            send_y = line_value[1]
            send_w = line_value[2]
            send_h = line_value[3]
            send_msg = "judge"
            #print(send_x, send_y, send_w, send_h, send_msg)
            #img.draw_string(0, 20, str(send_y), color=(255, 0, 0), scale=1.5)
            #img.draw_string(0, 20, str(send_y), color=(255, 0, 0), scale=1.5)
            send_data(JUDGE_MODE, send_x, send_y, send_w, send_h, send_msg)

    else:
        nothing_to_do_now = 0
        #print("none line")
    #lcd.display(img)


def collorring_judge():
    center_x = 0
    center_y = 0
    img = sensor.snapshot()
    img.morph(1, [+2, +1, +0, +1, +1, -1, +0, -1, -2])
    #img.morph(1, [+3, +1, +0, +1, +1, -1, +0, -1, -3])
    circles = img.find_circles(threshold = 8000, x_margin = 20, y_margin = 20, r_margin = 20, r_min = 30, r_max = 100, r_step = 5)
    if circles:
        for c in circles:
            img.draw_circle(c.x(), c.y(), c.r(), color=(255, 0, 0))
            img.draw_string(0, 20, str(c.x()), color=(255, 0, 0), scale=1.5)
            img.draw_string(50, 20, str(c.y()), color=(255, 0, 0), scale=1.5)
        send_x = c.x()
        send_y = c.y()
        send_w = c.r()
        send_h = 1
        send_msg = "circle_center"
        send_data(CIRCLE_CENTER, send_x,send_y,send_w,send_h,send_msg)
    else:
        nothing_to_do_now = 0
    #lcd.display(img)

#定义发送数据
send_x = 0
send_y = 0
send_w = 0
send_h = 0
send_msg = ""




##程序初始化部分--------------------------------------
#初始化LCD
lcd.init()
#以下是初始化传感器
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 100)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
#帧率时钟
clock = time.clock()
#方框参数
r = [(320//2)-(50//2), (240//2)-(50//2), 50, 50]

# 储存6种颜色的LAB阈值
color_thresholds = [
    #(46, 54, 46, 60, 2, 26),     #red
    (33, 47, 21, 56, -2, 23),    #new red color 2025-02-12
    #(60, 67, -44, -31, -7, 9),   #green
    (47, 58, -44, -20, -4, 15),   #new green color 2025-02-12
    #(47, 58, 14, 31, -65, -50),  #blue
    (35, 46, 14, 40, -71, -35), #new blue color 2025-02-12
]
color_strings = ['Redsth', 'Greensth', 'Bluesth', 'Yellow', 'purple']
recv_commond_qr_msg_start = 'qr_detect_start'
recv_commond_qr_msg_success = 'qr_detect_success'
recv_commond_color_msg_start = 'color_detect_start'
recv_commond_color_msg_success = 'color_detect_success'

#----------------------------------物体检测需要定义的参数
obj_name = ("red", "blue", "green")
img_obj = image.Image(size=(320,256))
anchor = (4.44, 4.34, 4.97, 4.69, 5.38, 5.09, 5.23, 5.33, 5.72, 5.44)
kpu = KPU()
kpu.load_kmodel('./sd/my_color_circle/det.kmodel')
kpu.init_yolo2(anchor, anchor_num=(int)(len(anchor)/2), img_w=320, img_h=240, net_w=320 , net_h=240 ,layer_w=10 ,layer_h=8, threshold=0.6, nms_value=0.3, classes=len(obj_name))
recv_commond_color_ring_msg_start = 'color_ring_start'
recv_commond_color_ring_msg_success = 'color_ring_success'
colorring_loop = False
fps = None
read_data = None
decoded_data = None

##色环位置判断------------------------------------------------
rho_data = 0
car_state = "None"
angle_data = None
line_data = None
judge_position_flag = False
#~~~~~~~~~~~~~######-主程序部分-######~~~~~~~~~~~~#
while(True):
    clock.tick()#用于计算帧率的函数，这里表示开始计时
    img = sensor.snapshot()#从传感器捕获一张图像
    read_data = serial.readline()
    if read_data:
        f_led.fill_onoff(1)
        try:
            #去除字节串中的换行符和回车符
            read_data = read_data.replace(b'\r', b'').replace(b'\n', b'')
            decoded_data = read_data.decode('utf-8')# 解码字节串为字符串
            #print(decoded_data)
            #=======================================================================
            #程序进入二维码识别判断
            #=======================================================================
            if decoded_data == recv_commond_qr_msg_start:
                #print("i receive mega 2560 qr start msg")
                for i in range(1):
                    #print("count:", i)
                    qr_detection_section()
            elif decoded_data == recv_commond_qr_msg_success:
                #print("success")
                send_x = 1
                send_y = 1
                send_w = 1
                send_h = 1
                send_msg ="qr_success"
                send_data(QR_MODE,send_x,send_y,send_w,send_h,send_msg)
            else:
                disnot_recv_qr_commond = 0

            #=======================================================================
            #程序进入颜色识别判断
            #=======================================================================
            if decoded_data == recv_commond_color_msg_start:
                #print("i receive mega 2560 color start msg")
                for i in range(1):
                    #print("count:", i)
                    color_detection_section()
            elif decoded_data == recv_commond_color_msg_success:
                #print("success")
                send_x = 1
                send_y = 1
                send_w = 1
                send_h = 1
                send_msg = "color_success"
                #send_data(COLOR_MODE,send_x,send_y,send_w,send_h,send_msg)
                #send_color_data(send_x,send_y,send_w,send_h,send_msg)
                send_data(COLOR_DATA_MODE,send_x,send_y,send_w,send_h,send_msg)
            else:
                disnot_recv_color_commond = 0

            #=======================================================================
            #程序进入颜色识别判断
            #=======================================================================
            if decoded_data == recv_commond_color_ring_msg_start:
                #print("i receive mega 2560 color ring start msg")
                for i in range(1):
                    #("count:", i)
                    color_ring_detect()
            elif decoded_data == recv_commond_color_ring_msg_success:
                print("colorring sucess")
                send_x = 1
                send_y = 1
                send_w = 1
                send_h = 1
                send_msg = "colorring_success"
                send_data(COLORRING_MODE, send_x,send_y,send_w,send_h,send_msg)
            else:
                disnot_recv_colorring_commond = 0

            #=======================================================================
            #程序进入色环位置判断
            #=======================================================================
            if decoded_data == "judge_car_start":
                for i in range(1):
                    position_judge()


            #=======================================================================
            #程序进入色环处圆环位置判断
            #=======================================================================
            if decoded_data == "change_car_position":
                for i in range(1):
                    collorring_judge()
        except ValueError:
            disnot_recv_colorring_commond = 0
            #print("无法解析数据:", read_data)      #处理无法解码或转换的情况
    else:
        f_led.fill_onoff(0)
    #显示在LCD上
    lcd.display(img)
    #打印帧率
    #print(clock.fps())


